
#include "IOoffs.h"

#include <string>
#include <iostream>
#include <cmath>
#include <cassert>
#include <vector>
#include <set>

using std::ifstream;
using std::string;

/// stores for each vertex the set of incident faces indices
std::vector<std::set<int> > facespervertex;
/// face normal array
std::vector<float>          facenormals;

void genFacesNormals() {
   
   for (int i=0; i<nfaces; ++i) {
      const float *p0 = &mesh_pos[mesh_faces[i*3+0]*4];
      const float *p1 = &mesh_pos[mesh_faces[i*3+1]*4];
      const float *p2 = &mesh_pos[mesh_faces[i*3+2]*4];
      float A[3] = {p1[0]-p0[0], p1[1]-p0[1], p1[2]-p0[2]};
      float B[3] = {p2[0]-p0[0], p2[1]-p0[1], p2[2]-p0[2]};
      
      /// Computes the cross product: A x B
      facenormals[i*3+0] = A[1]*B[2] - A[2]*B[1];
      facenormals[i*3+1] = A[2]*B[0] - A[0]*B[2];
      facenormals[i*3+2] = A[0]*B[1] - A[1]*B[0];
   }
}

void genVerticesNormals() {
   
   /// Compute the normal vector of each face
   genFacesNormals();
   
   for (unsigned int i=0; i<facespervertex.size(); ++i) {
      const std::set<int> &incidentfaces = facespervertex[i];
      std::set<int>::const_iterator it = incidentfaces.begin();
      float s[]={0.0,0.0,0.0}; int c=0;
      while (it != incidentfaces.end()) {
         const int &f = *it++; ++c;
         s[0] += facenormals[f*3+0];
         s[1] += facenormals[f*3+1];
         s[2] += facenormals[f*3+2];
      }
      s[0] /= (float)c;
      s[1] /= (float)c;
      s[2] /= (float)c;
      float sl = sqrt(s[0]*s[0] + s[1]*s[1] + s[2]*s[2]);
      
      if (sl > 0) {
         mesh_normals[i*4+0] = s[0]/sl;
         mesh_normals[i*4+1] = s[1]/sl;
         mesh_normals[i*4+2] = s[2]/sl;
         mesh_normals[i*4+3] = 1.0;
      }
      else {
         assert(false);
      }
   }
}

bool readOFFfile(std::ifstream& filein) {
   
   if (not filein.is_open()) {
      std::cout<<"I/O error: the file doesn't exist "<<std::endl;
      return false;
   }
   else {
      string type;
      int nt;
      float x, y, z; unsigned nv, v0,v1,v2;
      filein>>type;
      filein>>nvertices>>nfaces>>nt;
      assert(type == "OFF");
      assert(nvertices>0 and nfaces>0);
      
      mesh_pos = new float[4*nvertices];
      if (!mesh_pos) return false;
      mesh_normals = new float[4*nvertices];
      if (!mesh_normals) return false;
      mesh_faces = new unsigned int[3*nfaces];
      if (!mesh_faces) return false;
      
      for (int i=0; i<nvertices; ++i) {
         filein>>x>>y>>z;
         mesh_pos[i*4+0] = x;
         mesh_pos[i*4+1] = y;
         mesh_pos[i*4+2] = z;
         mesh_pos[i*4+3] = 1.0;
      }
      
      facespervertex.resize(nvertices);
      facenormals.resize(3*nfaces);
      
      for (int i=0; i<nfaces; ++i) {
         filein>>nv>>v0>>v1>>v2;
         assert(nv==3);
         mesh_faces[i*3+0] = v0;
         mesh_faces[i*3+1] = v1;
         mesh_faces[i*3+2] = v2;
         facespervertex[v0].insert(i);
         facespervertex[v1].insert(i);
         facespervertex[v2].insert(i);
      }
      
      genVerticesNormals();
      return true;
   }
}

/// Save the OFF model
/// @arg filename the file name
/// @return true if it succeed
bool saveOFFfile(std::ofstream& fileout) {
   
   if (not fileout.is_open()) {
      std::cout<<"I/O error "<<std::endl;
      return false;
   }
   else {
      assert(nfaces>0 and nvertices>0);
      
      fileout.precision(10);
      fileout.setf(std::ios::fixed, std::ios::floatfield);
      fileout<<"OFF"<<std::endl;
      fileout<<nvertices<<" "<<nfaces<<" "<<"0"<<std::endl;
      for (int i=0; i<nvertices; ++i) 
         fileout<<mesh_pos[i*4]<<" "<<mesh_pos[i*4+1]<<" "<<mesh_pos[i*4+2]<<std::endl;
      for (int i=0; i<nfaces; ++i) 
         fileout<<"3 "<<mesh_faces[i*3+0]<<" "<<mesh_faces[i*3+1]<<" "<<mesh_faces[i*3+2]<<std::endl;
      return true;
   }
}

bool readCPfile(std::ifstream& filein) {
   
   if (not filein.is_open()) {
      std::cout<<"I/O error: the file doesn't exist "<<std::endl;
      return false;
   } else {
      int id;
      std::vector<int> cpids; // control points id
      while(not filein.eof()) {
         filein>>id;
         cpids.push_back(id);
      }
      NCTRLPOINTS = cpids.size();
      idCP = new int[NCTRLPOINTS];
      for (unsigned int i=0; i<cpids.size(); ++i) {
         idCP[i] = cpids[i];
      }
      return true;
   }
}

bool readOFFfile(const char* f) {
   
   std::ifstream in(f);
   return readOFFfile(in);
}

bool saveOFFfile(const char* f) {
   
   std::ofstream out(f);
   return saveOFFfile(out);
}

bool readCPfile(const char* f) {
   
   std::string filename(f);
   std::string extensao =filename.substr(filename.size()-2,filename.size());
   if (extensao != "cp") return false;
   
   std::ifstream in(f);
   return readCPfile(in);
}
